package fag.core.collision;

import java.util.List;

import fag.core.geometry.Vector2D;
import fag.core.geometry.Winding;

/**
 * Classe de l'algorithme de polytope étendu (Expanded Polytope Algorithm). Calcule le vecteur normal et
 * la penetration de la collision.
 * @author lumz
 */
public class Epa {
	
	/**
	 * Résultat de l'algorithme Epa.
	 * @author lumz
	 */
	public static class Result {

		private Vector2D normal = null;
		private double penetration = 0;
		
		/**
		 * Constructeur par défault.
		 * @param normal Le vecteur normal de collision.
		 * @param penetration La pénétration de collision.
		 */
		private Result(Vector2D normal, double penetration) {
			this.normal = normal;
			this.penetration = penetration;
		}
		
		/**
		 * Accesseur de la pénétration.
		 * @return La pénétration de la collision.
		 */
		public double getPenetration() {
			return penetration;
		}
		
		/**
		 * Accesseur du vecteur normal de collision.
		 * @return Le vecteur normal de collision.
		 */
		public Vector2D getNormal() {
			return normal.clone();
		}
		
	}
	
	/**
	 * Classe qui encapsule les informations qui concernent l'arête du polytope la plus proche de l'origine.
	 * @author lumz
	 */
	private static class Feature {

		private int i = 0;
		private int j = 0;
		
		private Vector2D normal = null;
		private double distance = Double.MAX_VALUE;
	}
	
	private static final int MAX_LOOPS = 100;
	private static final double TOLERANCE = 0.01;
	
	/**
	 * Constructeur par défault.
	 */
	private Epa() {
	}
	
	/**
	 * Obtenir le résultat de l'algorithme.
	 * Attention : il faut au préalable avoir vérifier l'existance d'une collision grâce à l'algorithme Gjk.
	 * @param c1 Premier objet SupportMapping.
	 * @param c2 Deuxième objet SupportMapping.
	 * @param polytope Un 2-simplex qui englobe l'origine.
	 * @return Le résultat de l'algorithme Epa.
	 */
	public static Result getResult(Hitbox c1, Hitbox c2, List<Vector2D> polytope) {
		
		// Vérifier qu'il s'agit bien d'un 2-simplex
		assert(polytope.size() == 3);
		
		// Calculer le sens d'enroulement du simplex
		Winding winding = computeSimplexWinding(polytope);
		
		Feature closest = null;
		double distance = 0;
		
		int loops = 0;
		while(loops++ < MAX_LOOPS) {
			
			closest = computeClosestFeature(polytope, winding);
			
			Vector2D support = minkowskiSupportMapping(c1, c2, closest.normal);
			distance = support.dot(closest.normal);
			
			// Si la distance de la projection du point le plus éloigné sur le vecteur d'assistance est
			// à peu près la même que la distance du segment à l'origine alors terminer
			if(distance - closest.distance < TOLERANCE) {
				return new Result(closest.normal, distance);
			}
			// Sinon agrandir le polytope en ajoutant le nouveau sommet entre les deux sommets du segment le plus proche de l'origine
			else
			{
				polytope.add(closest.j, support);
			}
		}
		
		return new Result(closest.normal, distance);
	}
	
	/**
	 * Identifie la partie du polytope la plus proche de l'origine, et calcule sa normale et sa distance à l'origine.
	 * @param polytope Le polytope.
	 * @return Un objet qui encapsule toute ces informations.
	 */
	private static Feature computeClosestFeature(List<Vector2D> polytope, Winding winding) {
		
		Feature closest = new Feature();
		
		// Itération sur les couples de sommets adjacents du simplex (Arêtes)
		int simplexSize = polytope.size();
		for(int i = 0, j = 1; i < simplexSize; ++i, j = (i + 1) % simplexSize) {
			
			Vector2D a = polytope.get(i);
			Vector2D b = polytope.get(j);
			
			Vector2D normal = b.sub(a).normal(winding).normalize();
			double distance = normal.dot(a);
			
			// Si on trouve une arête plus proche de l'origine alors la garder
			if(distance < closest.distance) {
				closest.i = i;
				closest.j = j;
				closest.distance = distance;
				closest.normal = normal;
			}
		}
		
		return closest;
	}
	
	/**
	 * Calculer le sens d'enroulement du 2-simplex.
	 * @param simplex Le 2-simplex dont on souhaite calculer l'enroulement.
	 * @return Le sens de l'enroulement.
	 */
	private static Winding computeSimplexWinding(List<Vector2D> simplex) {
	
		Vector2D ac = simplex.get(0).sub(simplex.get(2));
		Vector2D ab = simplex.get(0).sub(simplex.get(1));
		
		return ac.det(ab) > 0 ? Winding.CLOCKWISE : Winding.COUNTERCLOCKWISE;
	}
	
	/**
	 * Calcule le point dans la différence de Minkowski le point dont la projection est la plus éloignée le long de d.
	 */
	private static Vector2D minkowskiSupportMapping(Hitbox c1, Hitbox c2, Vector2D d) {
		return c1.supportMapping(d).sub(c2.supportMapping(d.mul(-1)));
	}
}
